/**
 *  基于turtlesim的tf监听器
*/
#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <turtlesim/Spawn.h>
#include <tf/transform_listener.h>

class TurtleTfListener {
 public:
  TurtleTfListener() {
    // 请求服务/spawn进而创建turtle2
    ros::service::waitForService("/spawn");
    ros::ServiceClient add_turtle  = nh_.serviceClient<turtlesim::Spawn>("/spawn");
    turtlesim::Spawn spawn_srv;
    add_turtle.call(spawn_srv);

    // 创建控制turtle2速度的话题发布者
    turtle2_vel_pub_ = nh_.advertise<geometry_msgs::Twist>("/turtle2/cmd_vel", 10);
  }

  ~TurtleTfListener() {

  }

  void ListenTfAndCtrlTurtle2Vel(void) {
    ros::Rate loop_rate(10);

    while (nh_.ok()) {
      // 获取turtle1与turtle2坐标系之间的tf数据
      tf::StampedTransform trans;
      try {
        tf_listener_.waitForTransform("/turtle2", "/turtle1", ros::Time(0), ros::Duration(3.0));
        tf_listener_.lookupTransform("/turtle2", "/turtle1", ros::Time(0), trans);
      }
      catch(tf::TransformException& ex) {
        ROS_ERROR("%s", ex.what());
        ros::Duration(1.0).sleep();
        continue;
      }
      
      // 根据turtle1与turtle2坐标系之间的位置关系，发布turtle2的速度控制命令
      geometry_msgs::Twist vel_msg;
      vel_msg.angular.z = 4.0 * atan2(trans.getOrigin().y(), trans.getOrigin().x());
      vel_msg.linear.x = 0.5 * sqrt(pow(trans.getOrigin().x(), 2) + pow(trans.getOrigin().y(), 2));
      turtle2_vel_pub_.publish(vel_msg);
      // ROS_INFO("Publish Turtle2 Velocity msg[angular_z:%.2frad/s, linear_x:%.2fm/s]", vel_msg.angular.z, vel_msg.linear.x);
      loop_rate.sleep();
    }
  }

 private:
  ros::NodeHandle nh_;
  ros::Publisher turtle2_vel_pub_;
  tf::TransformListener tf_listener_;
};


int main(int argc, char** argv) {
  ros::init(argc, argv, "demo_tf_listener");
  
  TurtleTfListener turtlesim_tf_listen;

  turtlesim_tf_listen.ListenTfAndCtrlTurtle2Vel();

  return 0;
}
